COMMENT ⊗   VALID 00013 PAGES 
RECORD PAGE   DESCRIPTION
 00001 00001
 00003 00002	HOPEN:				OPENS THE HAND TO SET POINT
 00005 00003	HCLOSE:
 00007 00004	HEAD:	MOVEI K,HANDCHA
 00009 00005	GOTHER:	FSBR AC,TAC		HAND SERVO
 00010 00006	PLACE:				PLACE HAND ON TABLE
 00011 00007	NUDGE:	HRRE TAC,@STKPTR
 00013 00008	PATH:	TRO RUN+VZERO
 00015 00009	SAVE:	HRRE K,@STKPTR
 00018 00010	CENTER:	TRNN HCL
 00021 00011	SEARCH:	SETZM XSEL
 00024 00012	REFLEX:	MOVEI I,1
 00025 00013		XLIST
 00026 ENDMK
⊗;
HOPEN:				;OPENS THE HAND TO SET POINT
	TRNN HCL
	JRST[	MOVEI TAC,=480	;GO HERE FIRST TIME
		MOVEM TAC,HCNT	;MAX TIME TO SERVO THERE
		ADDI TAC,=60
		MOVEM TAC,COUNT
		HRLZ AC,@STKPTR	;REQUIRED OPENING
		MOVE MQ,[3.6]
		CAML AC,MQ
		MOVE AC,MQ	;NOT GREATER THAN 2.5
		MOVEM AC,RSET	;REQUIRED FINAL SET POINT
IFE USER,<	PUSHJ P,HEAD	;READS THE HAND>
		MOVE AC,HAND	;AND STORE IT HERE
		MOVEM AC,SET	;DYNAMIC SET POINT
		TRZ STCH	;TURN OFF TOUCH AND SAVE STATE
		MOVE TAC,[0.2]
		CAML AC,RSET
		MOVN TAC,TAC
		MOVEM TAC,DIR	;CHANGE OF SET POINT PER JIFFY
		TRO HCL		;SO THAT WE DON'T COME BACK HERE
		JUMPGE TAC,NXTJIF
		FSC TAC,-1	;IF CLOSING GO SLOWER
		MOVEM TAC,DIR
		SKIPE SOTCH	;AND RESTORE TOUCH
		TRO STCH
		JRST NXTJIF]
HSU:	PUSHJ P, HEAD		;INCREMENTS SETPOINT AND SETS D/A
				;LEAVES ARM DATAO IN TAC
IFE HANDWORK <
	JRST NEXT
>
	SOSGE HCNT
	JRST[	MOVEI AC,HTERR
		MOVEM AC,TRAJER
		POP P,AC
		JRST TOFF]
	SKIPE DIR
	JRST DAW		;IF NOT AT FINAL SET DRIVE ANYWAY
	MOVE AC,RSET
	FSBR AC,HAND		;CHECK ERROR TOLERANCE
	MOVM AC,AC
	FSBR AC,[0.05]
	JUMPLE AC,[	TRZ HCL
			TDZ DATWD,HANDAT
			JRST NEXT]
DAW:	
IFN TRACK,<SKIPN WALK>
	DATAO ARM,DATWD		;DRIVE HAND
	JRST NXTJIF

HCLOSE:
	TRNN HCL		;ONLY ONCE
	JRST[	TRZ STCH	;TURN OFF TOUCH AND SAVE STATE
		MOVEI TAC,=240
		MOVEM TAC,COUNT
		HRLZ AC,@STKPTR	;MINIMUN OPENING
		MOVEM AC,MINDST
IFE USER,<	PUSHJ P,HEAD	;READ HAND>
		MOVE AC,HAND
		MOVEM AC,SET
		MOVE TAC,[-0.10]
		MOVEM TAC,DIR	;CHANGE OF SET POINT PER JIFFY
		MOVSI AC,(4.0)
		MOVNM AC,RSET	;FINAL SET POINT -1 SO AS TO SQUEEZE
		TRO HCL
		JRST NXTJIF]
BB:	PUSHJ P,HEAD
IFE HANDWORK <
	JRST ER2
>
IFN TRACK,<SKIPN WALK>
	DATAO ARM,DATWD		;DRIVE HAND
	MOVE AC,HERR
	CAMG AC,[0.50]
	JRST NXTJIF		;IF ERROR LESS THAN 1 KEEP ON
ER2:	TRZ HCL
	TDZ DATWD,HANDAT
	SKIPE SOTCH		;RESTORE TOUCH
	TRO STCH
	MOVE AC,MINDST
	CAMG AC,HAND
	JRST NEXT
	MOVEI 1,GRASER		;IF LESS THAN MINOPENING GIVE ERROR
	MOVEM 1,TRAJER
	POP P,1
	JRST  TOFF

HEAD:	MOVEI K,HANDCHA
IFE USER,<
	PUSHJ P,PREAD		;READ HAND CHANNEL
	LDB AC,SNUM
	ANDI DACVAL,7777
	ADDI AC,(DACVAL)	;ADD ALL THREE READINGS TOGETHER
	FSC AC,220
	FMPR AC,HSCALE
	FADR AC,HOFF
	MOVEM AC,HAND		;OPENING IN INCHES
>
IFN USER,<
	MOVE AC,FUNCT
	CAIN AC,1
	JRST[	SKIPN DIR
		JRST[	MOVE AC,RSET
			JRST SFH]
		MOVE AC,HAND
		FADR AC,DIR
		JRST SFH]
	MOVE AC,HAND
	FADR AC,DIR
	CAMG AC,MINDST
	MOVE AC,MINDST
SFH:	MOVEM AC,HAND
>
IFN REPORT,<
	PUSH DATA,[<SIXBIT/HAND/>+1]
	PUSH DATA,HAND
>
	SKIPN TAC,DIR
	JRST[	MOVE TAC,SET
		JRST GOTHER]
	FADRB TAC,SET 		;INCREMENT SET POINT
	FSBR TAC,RSET	;NEGATIVE REMAINS TO GO
	FMPR TAC,DIR
	JUMPGE TAC,[	MOVE TAC,RSET	;GOT THERE
			MOVEM TAC,SET
			SETZM DIR
			JRST GOTHER]
	MOVE TAC,SET
GOTHER:	FSBR AC,TAC		;HAND SERVO
	EXCH AC,HERR
	FSBR AC,HERR
	FMPR AC,KVH
	MOVN TAC,HERR
	FMPR TAC,KEH
	FADR AC,TAC
	TDO DATWD,[HANDAT:XWD 4000,60]
	JUMPGE AC,.+2
	TRZ DATWD,40
	MOVM AC,AC
	FIX AC,211000
	CAILE AC,776000
	MOVEI AC,776000
	TRC AC,400000
	HRLI AC,6
IFN DEB<
	MOVEM AC,HANDAW
>
IFN TRACK,<SKIPN WALK>
	DATAO WIDTH,AC
	POPJ P,
HSCALE:	1.32
HOFF:	-0.81
DIR:	0			;CHANGE OF SETPOINT PER JIFFY
SET:	0			;DYNAMIC SET POINT
RSET:	0			;FINAL SET POINT
MINDST:	0			;MIN OPENING FOR CLOSE
HERR:	0			;HAND POSITION ERROR
KEH:	5.0			;ERROR GAIN
KVH:	10.0			;VELOCITY GAIN
IFN DEB<
HANDAW:	0
>

PLACE:				;PLACE HAND ON TABLE
	TRNN DROP		;ONLY COME HERE ONCE
	JRST[	TRO DROP+RUN+VZERO
		MOVEI 1,=360
		MOVEM 1,COUNT
		HRRE 1,@STKPTR
		ADDI 1,(CBUF)
		HRLI 1,(1)
		HRRI 1,DELTH
		BLT 1,DELTH+5	;CHANGE OF THETA PER JIFFY
		MOVEI 1,=12
		MOVEM 1,HCNT	;DON'T STOP FOR THE FIRST 12 JIFFIES
		MOVE DATWD,[770000252502]
		HRRZM DATWD,FBI
		JRST NXTJIF]
	SOSL HCNT
	JRST NXTJIF
IFN USER,<JRST PLACED>
EXL:	MOVM AC,ET0+1
	CAML AC,PLER
	JRST PLACED
	JRST NXTJIF
PLACED:	SETZM FUNCT
	JRST RUDONE
PLER:	0.6

NUDGE:	HRRE TAC,@STKPTR
	ADDI TAC,5(CBUF)
	MOVEI I,5
NUL1:	MOVE AC,(TAC)
	MOVEM AC,DELTH(I)
	TRNE NSET
	JRST[	FADRM AC,TFF(I)
		MOVN AC,AC
		FADRM AC,DTH(I)
		JRST .+1]
	SOJ TAC,
	SOJGE I,NUL1
	TRZ NSET
	HRRE 1,@STKPTR
	ADDI 1,6(CBUF)
	PUSHJ P,UPDC
	HRRE 1,@STKPTR
	ADDI 1,6(CBUF)
	MOVE 2,7(1)
	MOVEM 2,NTICKS
	ADDI 2,=60
	MOVEM 2,COUNT
	TRO INCREM+RUN+VZERO
	SETZM TICKS
	TRZE BOTH
	JRST[	AOS AC,STKPTR
		HLRE AC,(AC)
		MOVEM AC,FUNCT
		JRST .+2]
	SETZM FUNCT
	PUSHJ P,NEXT
	JRST NXTJIF


STOP:	HRRE 1,@STKPTR
	ADDI 1,(CBUF)
	HRLI 1,(1)
	HRRI 1,STQ
	BLT 1,STQ+5
	MOVEI I,5
	SETZ AC,
DSTQ:	MOVE TAC,STQ(I)
	FDVR TAC,F0(I)
	MOVM MQ,TAC
	CAMGE MQ,[0.2]
	SETZB TAC,STQ(I)
	MOVEM TAC,STQ(I)
	FMPR TAC,TAC
	FADR AC,TAC
	SOJGE I,DSTQ
	MOVEM AC,SDTQ
	SETZM ASTPP
	TRO STP
	JRST NEXT

TOOL:	TRNN HCL
	JRST[	HRRE TAC,@STKPTR
		MOVM AC,TAC
		CAIL AC,100
		MOVEI AC,77
		JUMPGE TAC,TDIP
		TRC AC,177
	TDIP:	TRO AC,200
		HRRM AC,TDAT
		TRO HCL
		JRST .+1]
	SKIPG COUNT
	JRST[	SETZM TDAT
		DATAO DEVT,TDAT
		TRZ HCL
		JRST NEXT]
	DATAO DEVT,TDAT
	JRST NXTJIF
TDAT:	0

PATH:	TRO RUN+VZERO
	AOS DOING	;INCREASE TRAJECTORY NUMBER
	HRRE TAC,@STKPTR
	ADDI TAC,(CBUF)
	HRRZ AC,(TAC)
	MOVEM AC,NTICKS	;RUN TIME
	MOVEI AC,=1800
	MOVEM AC,COUNT
	SETZM LOOP
	MOVE TAC,[XWD LOOP,LOOP+1]
	BLT TAC,LOOP+9
	MOVEI I,5
	HRRE K,@STKPTR
	ADDI K,34
SUL:	SUBI K,4
	HRLZM K,TJ(I)
	HRLZM K,TN(I)
	MOVEI TAC,TJ(I)
	MOVEM TAC,TP(I)
	SETZM DTH(I)
	SOJGE I,SUL
	PUSHJ P,EVAL
	TRZ INCREM
	MOVEI I,5
MOVBK:	MOVE AC,TH(I)
	MOVEM AC,TF(I)
	MOVEM AC,T0(I)
	FSBR AC,TFF(I)
	CAIN I,5
	JRST[	MOVSI MQ,(180.0)
		CAML AC,MQ
		JRST[SETROT:FSC MQ,1
			MOVN MQ,MQ
			FADRM MQ,TH+5
			FADRM MQ,ROT6
			JRST MOVBK]
		MOVN MQ,MQ
		CAMG AC,MQ
		JRST SETROT
		JRST ADDTF]
ADDTF:	MOVEM AC,DTH(I)
	MOVN AC,AC
	FADR AC,DDTH(I)
	SETZM DDTH(I)
	MOVEM AC,DELTH(I)
	SETZM TD(I)
	SOJGE I,MOVBK
	PUSHJ P,EVAL
	PUSHJ P,NXTH
	SETZM TICKS
	TRZE BOTH
	JRST[	AOS AC,STKPTR
		HLRE AC,(AC)
		MOVEM AC,FUNCT
		JRST .+2]
	SETZM FUNCT
	JRST NXTJIF

SAVE:	HRRE K,@STKPTR
	ADDI K,(CBUF)
	MOVE TAC,(K)
	JUMPLE TAC,.+2
	CAILE TAC,12
	JRST[	MOVEI TAC,SAVERR
		MOVEM TAC,TRAJER
		JRST TOFF]
	IMULI TAC,3
	ADDI TAC,DTHS
	MOVEI J,2
SAL1:	MOVEI I,5
	SOJ TAC,
	SETZ AC,
SAL2:	AOJ K,
	MOVE MQ,DTH(I)
	FMPR MQ,(K)
	FADR AC,MQ
	SOJGE I,SAL2
	MOVEM AC,(TAC)
	SOJGE J,SAL1
	JRST NEXT

RESTORE:HRRE K,@STKPTR
	ADDI K,(CBUF)
	MOVE TAC,(K)
	JUMPLE TAC,.+2
	CAILE TAC,12
	JRST[	MOVEI TAC,SAVERR
		MOVEM TAC,TRAJER
		JRST TOFF]
	IMULI TAC,3
	ADDI TAC,DTHS-1
	ADDI K,22
	MOVEI I,5
REL1:	MOVE AC,(TAC)
	FMPR AC,(K)
	MOVE MQ,-1(TAC)
	FMPR MQ,-1(K)
	FADR AC,MQ
	MOVE MQ,-2(TAC)
	FMPR MQ,-2(K)
	FADR AC,MQ
	FADRM AC,DDTH(I)
	SUBI K,3
	SOJGE I,REL1
	JRST NEXT

TIMFAC: 0.3
	0.6
	3.0
	0.3
	0.3
	0.2

SET.ARM:HRRE I,@STKPTR
	ADDI I,(CBUF)
	HRRE AC,(I)
	SOJL AC,.+2
	CAIL AC,11
	JRST[	MOVEI TAC,SAVERR
		MOVEM TAC,TRAJER
		JRST TOFF]
	IMULI AC,3
	HRRI TAC,DTHS(AC)
	HRRZI MQ,2(TAC)
	HRLI TAC,1(I)
	BLT TAC,(MQ)
	JRST NEXT

SLAVE:	MOVEI K,15B23
	MOVEI I,3
SLRP:	PUSHJ P,PREAD
	LDB AC,SNUM
	SUBI AC,4000
	MOVEM AC,POT(I)
	SOJGE I,SLRP
	TRNN HCL
	JRST[	TRO RUN+VZERO
		MOVE DATWD,[770000252502]
		HRRZM DATWD,FBI
		TRO HCL
		MOVEI AC,=6000
		MOVEM AC,COUNT
		MOVEI I,3
	SLZL:	MOVE AC,POT(I)
		MOVEM AC,POTZ(I)
		SOJGE I,SLZL
		JRST .+1]
	MOVEI I,3
SLCH:	MOVE AC,POT(I)
	SUB AC,POTZ(I)
	JUMPGE AC,[SUBI AC,400
		JUMPGE AC,SLST
		JRST SLZA]
	ADDI AC,400
	JUMPL AC,SLST
SLZA:	SETZ AC,
SLST:	FSC AC,213
	MOVEM AC,POT(I)
	SOJGE I,SLCH
	SKIPE POT+3
	JRST[	TRZ HCL
		TRO FINAL
		SETZM FUNCT
		JRST NXTJIF]
	HRRE K,@STKPTR
	ADDI K,21(CBUF)
	MOVEI I,5
SLL2:	MOVE AC,POT+2
	FMPR AC,(K)
	MOVE MQ,POT+1
	FMPR MQ,-1(K)
	FADR AC,MQ
	MOVE MQ,POT
	FMPR MQ,-2(K)
	FADR AC,MQ
	FADRM AC,DTH(I)
	SUBI K,3
	SOJGE I,SLL2
	JRST NXTJIF
POT:	BLOCK 4
POTZ:	BLOCK 4

CENTER:	TRNN HCL
	JRST[	TRO HCL
		MOVEI TAC,=600
		MOVEM TAC,COUNT
		HRLZ AC,@STKPTR	;MINIMUN OPENING
		MOVEM AC,MINDST
		MOVSI AC,(4.0)
		MOVNM AC,RSET
IFE USER,<	PUSHJ P,HEAD>
		MOVE AC,HAND
		MOVEM AC,SET
		TRZ STCH
		MOVE TAC,[-0.02]
		MOVEM TAC,DIR
		JRST NXTJIF]
	PUSHJ P,HEAD
IFE HANDWORK,<JRST ER2>
	DATAO ARM,DATWD
	TRNN DROP
	JRST[	MOVEI I,1
	CFT:	MOVE AC,OBS(I)
		JUMPG AC,[SETZM TOT
			SKIPN I
			AOS TOT
			TRO DROP+RUN+VZERO
			MOVE TAC,[-0.01]
			MOVEM TAC,DIR
			HRRE TAC,@STKPTR
			ADDI TAC,(CBUF)
			MOVE AC,(TAC)
			MOVEM AC,MINDST
			AOJ TAC,
			MOVEI I,DELTH
		OT:	MOVE AC,(TAC)
			FMPR AC,[0.005]
			SKIPN TOT
			MOVNS AC
			MOVEM AC,(I)
			AOJ TAC,
			CAIGE I,DELTH+5
			AOJA I,OT
			MOVE TAC,[XWD 770000, 252500]
			HRRZM TAC,FBI
			IOR DATWD,TAC
			MOVEI I,4
		GETGO:	PUSHJ P,NXTH
			SOJGE I,GETGO
			JRST NXTJIF]
		SOJGE I,CFT
		JRST NXTJIF]
	MOVE TAC,TOT
	SKIPG OBS(TAC)
	JRST NXTJIF
	TRZ DROP+RUN+FINAL
	PUSHJ P,SETSET
	TDZ DATWD,[XWD 770000,252500]
	MOVE TAC,DIR
	FSC TAC,3
	MOVEM TAC,DIR
	MOVEI TAC,2
	MOVEM TAC,FUNCT
	JRST NXTJIF]
TOT:	0

WOBBLE:	HRLZ AC,@STKPTR
	MOVEM AC,WOBMAG
	MOVEI AC,1
	MOVEM AC,WOBCNT+2
	MOVEM AC,WOBCNT+1
	MOVEI AC,6
	MOVEM AC,WOBCNT
	MOVN AC,WOBMAG
	FADRM AC,DTH+3
	TRO WOB
	JRST NEXT
WOBMAG:	0
WOBCNT:	BLOCK 3
SIN:	 0.0
	 0.30902
	 0.58779
	 0.80902
	 0.95106
	 1.0
	 0.95106
	 0.80902
	 0.58779
	 0.30902
	 0.0
	-0.30902
	-0.58779
	-0.80902
	-0.95106
	-1.0
	-0.95106
	-0.80902
	-0.58779
	-0.30902
SEARCH:	SETZM XSEL
	SETZM SCOUNT
	SETZM TCOUNT
	HRRE 1,@STKPTR
	ADDI 1,(CBUF)
	HRLI 1,(1)
	HRRI 1,XDEL
	BLT 1,YDEL+5
	SETZM SDTH
	MOVE AC,[XWD SDTH,SDTH+1]
	BLT AC,SDTH+5
	JRST NEXT

AOJI:	MOVEI I,5
	MOVE K,SCOUNT
	SOSG J,TCOUNT
	JRST[	SKIPN XSEL
		AOS K,SCOUNT
		SETCMM XSEL
		MOVEI J,(K)
		MOVEM J,TCOUNT
		JRST .+1]
	SKIPE XSEL
	JRST[IX:MOVE AC,XDEL(I)
		TRNN K,1
		MOVN AC,AC
		FADRB AC,SDTH(I)
		FADRM AC,DDTH(I)
		SOJGE I,IX
		JRST NJ]
IY:	MOVE AC,YDEL(I)
	TRNN K,1
	MOVN AC,AC
	FADRB AC,SDTH(I)
	FADRM AC,DDTH(I)
	SOJGE I,IY
NJ:	HRRE TAC,@STKPTR
	ADD TAC,STKPTR
	SOJ TAC,
	HRRM TAC,STKPTR
	JRST NEXT

TCOUNT:	0
SCOUNT:	0
XSEL:	0
XDEL:	BLOCK 6
YDEL:	BLOCK 6
SDTH:	BLOCK 6

HOME:	HRRE TAC,@STKPTR
	ADDI TAC,5(CBUF)
	MOVEI I,5
HLN:	MOVE AC,(TAC)
	FADR AC,6(TAC)
	MOVEM AC,TFF(I)
	FSBR AC,T0(I)
	MOVNM AC,DTH(I)
	FADR AC,DDTH(I)
	SETZM DDTH(I)
	MOVEM AC,DELTH(I)
	SOJ TAC,
	SOJGE I,HLN
	MOVEI I,5
	MOVE TAC,DELTH+5
	CAML TAC,[180.0]
	JRST[	FSBR TAC,[360.0]
		JRST .-1]
	CAMGE TAC,[-180.0]
	JRST[	FADR TAC,[360.0]
		JRST .-1]
	MOVEM TAC,DELTH+5
	HRRE AC,@STKPTR
	ADDI AC,=12(CBUF)
	PUSHJ P,UPDC
	HRRE AC,@STKPTR
	ADDI AC,=12(CBUF)
	MOVE TAC,7(AC)
	MOVEM TAC,NTICKS
	MOVEI I,5
	SETZ MQ,
HNCCA:	MOVM TAC1,DELTH(I)
	FMPR TAC1,TIMFAC(I)
	CAML TAC1,MQ
	MOVE MQ,TAC1
	SOJGE I,HNCCA
	FADR MQ,[20.0]
	FIX MQ,233000
	CAML MQ,NTICKS
	MOVEM MQ,NTICKS
	ADDI MQ,=60
	MOVEM MQ,COUNT
	TRO INCREM+RUN+VZERO+NSET
	SETZM TICKS
	MOVE AC,STKPTR
	TRNE BOTH
	AOJ AC,
	HLRZ AC,1(AC)
	CAIE AC,5
	TRZ NSET
	TRZE BOTH
	JRST[	AOS AC,STKPTR
		HLRE AC,(AC)
		MOVEM AC,FUNCT
		JRST .+2]
	SETZM FUNCT
	PUSHJ P,NXTH
	JRST NXTJIF

REFLEX:	MOVEI I,1
	MOVEI K,65B23
REX:
IFE USER,<
	PUSHJ P,PREAD
	LDB AC,[POINT 12,DACVAL,11]
	ADD AC,REF(I)
	MOVEM AC,OBS(I)
>
RMOR:	SOJGE I,REX
	TRNN STCH
	POPJ P,
	MOVEI I,1
SIT:	MOVE AC,OBS(I)
	JUMPG AC,[	LSH I,3
			IORI I,TOUCH
			MOVEM I,TRAJER
			POP P,I
			JRST TOFF]
	SOJGE I,SIT
	POPJ P,

REF:	-1000
	-1000
	BLOCK 4
IFN SIMU,<
	OBS-1
ARM.TO:	.+4
>
OBS:	0
	0
SOTCHS:	0

SETCH:	MOVE AC,SOTCH
	MOVEM AC,SOTCHS
	HRLE AC,@STKPTR
	MOVEM AC,SOTCH
	JUMPE AC,NEXT
	TRO STCH
	JRST NEXT

	XLIST
	LIT
	LIST
IFE SIMU,<END START>
IFN SIMU,<END>